function [uc_opt_, u_opt_, P_nom, alpha_nom] = TubeNMPC_Controller(ref, g, g_sim, u_r, k, ocp_nmpc, u_opt) %#codegen
% This function
% Name          :TubeNMPC_Controller
% Descriptions  :使用TubeNMPC方法控制，得到速度的控制输出
% Argument(s)   :g:当前位姿xyθ; ref:参考位姿序列xyθ; u_r:参考输入
% Return(s)     :u_(1):得到的速度v;u_(2):得到的角速度Ω
% Author        :LHP,on April 2nd,2024

%% Problem formulation
    % 定义NMPC的参数
    N = 35;                                     % 预测步长
    T = 21.0;                                   % 预测总时长[s]
    dt = T/N;
    Q = diag([1.0, 1.0, 1.0]);                  % 状态权重矩阵
    R = diag([1.0, 1.0]);                       % 输入权重矩阵
    nu = 2;

%% Disturbance
    Bw = [1,0,0;0,1,0;0,0,3];                   % 扰动输入矩阵
    
    wmax_x = 0.006;                             % 状态x扰动的最大值0.01m/s 0.006
    wmax_y = 0.006;                             % 状态y扰动的最大值0.01m/s
    wmax_theta = 0.006;                         % 状态theta扰动的最大值0.03rad/s
    
    wmax = [wmax_x; wmax_y; wmax_theta];
    wmax_norm = norm(wmax);
    
%% Initial settings
    uM = [0.5; 0.5];                            % 控制输入u的约束条件v_max,w_max
    xM = [0.05; 0.05; 0.05];                       % 状态误差deltax的约束 xM = [0.02; 0.02; 0.02]; [0.07; 0.07; 0.15]
 
    % 离散线性化差速机器人误差模型得到A和B
    [A,B] = sim_linear_errmodel(u_r(1), u_r(2), dt); 

%% Caculate the robust invariant set (RIS)=鲁棒控制不变集

    [P1, K, lambda0, mu] = NMPC_get_ris_new (A, B, Bw); 
    % mu
    % % 图: 最小鲁棒正不变集的形状
    % %draw_ellip(P1, mu * wmax_norm^2/lambda0, 'b') % 绘制椭圆
    % %hold on
     
    % 在鲁棒控制不变集范围内的受最大程度干扰情况下控制输入u的优化计算问题
    yalmip('clear');
    x = sdpvar(3,1);                                        % 定义优化变量
    % 约束条件:确保x属于robust invariant set的前提下进行优化
    const = x' * P1 * x <= (mu * wmax_norm^2 / lambda0); 
    % 目标函数
    obj1 = K * x; 
    % 最小化目标函数obj1，并且满足约束条件const
    ops = sdpsettings('solver','Mosek', 'verbose', 0);      % fmincon/sdpt3/Mosek都可以
    optimize(const, obj1, ops);                             % 优化
    u_0 = value(obj1);                                      % 获取优化结果:鲁棒不变集得到的控制量约束
 
%% Get the terminal region Xf and terminal penalty for nominal MPC=终端最大鲁棒正不变集

    % alphaM被用作名义MPC中终端区域和终端权重的上界
    alphaM = 5;                                            % 越小，tube越小 alphaM = 5
    
    [P_nom, K0_nom, alpha_nom] = NMPC_get_max_terminal_tube_new(Q, R, uM + u_0, xM, alphaM, A, B);
    
    % 图: 最大终端不变集范围
    %draw_ellip(P_nom, alpha_nom, 'g')
    %legend({'mRIS','MPI'},'Orientation','horizontal');
       
%% NMPC Optimization problem using acados
    
    % 更新状态
    x0 = g_sim;                                                     % g_sim
    tilde_x = g - g_sim;                                            % 误差反馈 g - g_sim
    
    % 设置初始状态约束
    ocp_nmpc.set('constr_x0', [x0;zeros(nu,1)]);                    % 设置初始条件 x(0) = x0.

    % ext_cost
    % for j = 0:(N-1)                              
    %  ocp_nmpc.set('p', ref(:, j+1), j);                           % 设置y_ref
    % end
    
    % nonlinear_ls
    for j = 0:(N-2)               
        ocp_nmpc.set('cost_y_ref', [ref(:, j+1);zeros(nu,1)], j);   % for the stage cost y_ref = [x;u]           
        % tube    
        ocp_nmpc.set('constr_lbx', [-uM(1) - u_0(1), -uM(2) - u_0(2)], j+1); % 1 - N-1
        ocp_nmpc.set('constr_ubx', [ uM(1) + u_0(1),  uM(2) + u_0(2)], j+1);
    end
    ocp_nmpc.set('cost_y_ref_e', [ref(:, N);zeros(nu,1)], N-1);     % for the terminal cost y_ref_e
    % tube
    W1 = Q/P_nom;
    W2 = R/P_nom(1:2,1:2);
    ocp_nmpc.set('cost_W', blkdiag(W1,W2));

    % 求解优化问题
    ocp_nmpc.solve();
    status = ocp_nmpc.get('status');                                % 0 - success
    if status ~= 0
        % borrowed from acados/utils/types.h
        %statuses = {
        %    0: 'ACADOS_SUCCESS',
        %    1: 'ACADOS_NAN_DETECTED',
        %    2: 'ACADOS_MAXITER',
        %    3: 'ACADOS_MINSTEP',
        %    4: 'ACADOS_QP_FAILURE',
        %    5: 'ACADOS_READY'
        %error(sprintf('acados returned status %d in closed loop iteration %d. Exiting.', status, k)); 
        % bug
    else
        % 提取最优控制输入
        % du = ocp_nmpc.get('u', 0);                                    % 获取控制问题第一个时间步的输入变量deltau
        % u_opt = last_u_opt_ + du*dt;                                  % v=v0+a*dt,不知道为什么不对??
    
        u_opt = ocp_nmpc.get('x',1);                    
        u_opt = u_opt(4:5);                                             % 获取控制问题第二个时间步的速度状态量u
    end

    % tube
    uc_opt = u_opt + K * tilde_x;
    
%% TubeNMPC控制量输出
    uc_opt_ = uc_opt;
    u_opt_ = u_opt;

end